
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Import/RobotImporterFactory.h>

#include <Inventor/Qt/SoQt.h>
#include <QFileInfo>

#include <boost/extension/shared_library.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <string>
#include <map>
#include <iostream>
#include <fstream>
#include <sstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderC3D.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessor.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include <MMM/Motion/Legacy/Converter/ConverterFactory.h>


using std::cout;
using std::endl;
using namespace VirtualRobot;

int main(int argc, char *argv[])
{
    cout << " --- MMM motion connector --- " << endl;
    cout << "join two motions from two files into one motion" << endl;

    // we are expecting three arguments, and that is the filename of the file to convert
    if (argc<4)
    {
        cout << "Missing filename as first argument, aborting..." << endl;
        cout << "Syntax: joinMotions %motionfile1_in %motionfile2_in %targetmotionfile_out" << endl;
        //MMM_ERROR << endl << "Could not process command line, aborting..." << endl;
        return -1;
    }
    //c.print();


    std::string filename1 = std::string(argv[1]);
    std::string filename2 = std::string(argv[2]);
    std::string filenameOut = std::string(argv[3]);

    cout << "Filenames provided for cutting: [" << filename1 << ", " << filename2 << "] -> [" << filenameOut << "]" << endl;

    cout << "Checking mmm files ..." << std::endl;
    if (!filename1.empty() && !filename2.empty())
    {
        std::vector<std::string> filenames;
        filenames.push_back(filename1);
        filenames.push_back(filename2);

        for(size_t i = 0; i < filenames.size(); i++)
        {
            std::ostringstream converter;
            converter << i;
            std::string istr = converter.str();

            std::string filename = filenames[i];
            // input file 1
            QFileInfo fileInfo(filename.c_str());
            std::string suffix(fileInfo.suffix().toLatin1());

            // transform suffixes to lowerCase
            std::transform(suffix.begin(), suffix.end(), suffix.begin(), ::tolower);

            // For now, we just concatenate MMM motion files, so we check for correct suffixes
            if ((suffix.compare("xml")!=0))
            {
                cout << "Expected MMM Motion file " << istr << " as input (.xml). Aborting..." << endl;
                return -1;
            }
            // load input files
            MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
            cout << "Checking motions in file " << istr << " :" << endl;
            std::vector < std::string > motionNames = r->getMotionNames(filename);

            // there should be at least one motion in every file
            if (motionNames.size() == 0)
            {
                MMM_ERROR << "no motions in file " << istr << endl;
                return -1;
            }
            else
                cout << "Number of Motions in file " << istr << " : " << motionNames.size() << endl;

        }
    }


    cout << "Joining two motions ..." << std::endl;

    MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
    MMM::LegacyMotionPtr motion1 = r->loadMotion(filename1);
    MMM::LegacyMotionPtr motion2 = r->loadMotion(filename2);

    MMM::LegacyMotionPtr motion = motion1->copy();
    motion->joinMotion(motion2);

    // xml-string for all motions
    std::string content = motion->toXML();

    // save this xml-string to the output file
    if (!MMM::XML::saveXML(filenameOut, content))
    {
        MMM_ERROR << " Could not write to file " << filenameOut << endl;
    }

    return 0;
}
